#include "encoder_calibration.h"

#include "motor_a_tim2_pwm.h"
#include "motor_b_tim4_pwm.h"

#include "motor_a_encoder_config.h"
#include "motor_b_encoder_config.h"

#include "delay.h"
#include "stdio.h"


/*
 * @brief   传感器零角度校准
 *
 * @param   need	是否需要校准。1：需要；0：不需要。
 *
 * @return  none 
 */
void MAGNETIC_ENCODER_CALIBRATION(uint8_t need)
{
	if(need)
	{
		float angleA, angleB;
		
		// 1. 将电机A旋转至A轴
		MOTORA_SetChannelAHighLeaveTime(70);
		MOTORA_SetChannelBHighLeaveTime(40);
		MOTORA_SetChannelCHighLeaveTime(40);
		
		// 2. 将电机B旋转至A轴
		MOTORB_SetChannelAHighLeaveTime(70);
		MOTORB_SetChannelBHighLeaveTime(40);
		MOTORB_SetChannelCHighLeaveTime(40);
		
		delay_ms(500);
		
		// 3. 读取角度
		for (uint8_t i = 0; i < 10; i++)
		{
			angleA = MOTORA_GetEncoderAngle();
			angleB = MOTORB_GetEncoderAngle();
			
			HAL_Delay(100);
			
			printf("encoder calibration: %f, %f \r\n", angleA, angleB);
		}
		
		// 4. 失能电机
		MOTORA_SetChannelAHighLeaveTime(0);
		MOTORA_SetChannelBHighLeaveTime(0);
		MOTORA_SetChannelCHighLeaveTime(0);
		MOTORB_SetChannelAHighLeaveTime(0);
		MOTORB_SetChannelBHighLeaveTime(0);
		MOTORB_SetChannelCHighLeaveTime(0);
		
		while(1)
		{
			// 得到数据后，修改
			// motor_a_encoder_config.c
			// motor_b_encoder_config.c
			// 文件中的零偏移角度
			// 和
			// motor_a_adc1_current.h
			// motor_b_adc2_current.h
			// 文件中的静态电流值
			
			// 然后在 app.c文件中的APP_Init函数中，将
			// STATIC_CURRENT_CALIBRATION(1);		// 静态电流校准
			// MAGNETIC_ENCODER_CALIBRATION(1);	// 传感器校准
			// 中的参数改为0，重新启动即可
		}
	}
}


